// Copyright 2006-2012 Brendan Burns
//
// Permission is hereby granted, free of charge, to any person obtaining
// a copy of this software and associated documentation files (the
// "Software"), to deal in the Software without restriction, including
// without limitation the rights to use, copy, modify, merge, publish,
// distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to
// the following conditions:
//
// The above copyright notice and this permission notice shall be
// included in all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND,
// EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
// MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
// NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE
// LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
// OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
// WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
#include <stdio.h>
#include <stdlib.h>
#include <sys/time.h>
#include <unistd.h>
#include "nmc_low_level.h"
#include "nmc_status.h"
#include "nmc.h"
#define DEBUG

int nmc_init(int baud, const char *device, nmc_port **port) {
  int i;
  int nummod=0;
  int fd;
  (*port) = nmc_connect(device, baud, &nummod);
  for (i=1;i<=nummod;i++) {
    printf("Initializing module %d\n", i);
    nmc_init_module(*port, i);
#ifdef DEBUG
    printf("%d: %d %d\n", 
	   i, (*port)->modules[i-1].type, (*port)->modules[i-1].version);
#endif
  }
  return nummod;
}

/**
 * @see PicServo-SC Manual Pg. 31
 * \param ix The index of the board
 * \param Kp uint16_t
 * \param Ki uint16_t
 * \param Kd uint16_t
 * \param integration_limit uint16_t
 * \param output_limit 0-255
 * \param current_limit 0-255
 * \param error_limit uint16_t
 * \param rate_divisor
 * \param deadband_compensation
 **/
void nmc_set_full_pid_parameters(nmc_port *port, 
				 uint ix, int Kp, int Ki, int Kd, 
				 int integration_limit, int output_limit, 
				 int current_limit,
				 int error_limit, int rate_divisor, 
				 int deadband_compensation)
{
  nmc_set_gain(port, ix,Kp,Ki,Kd,integration_limit,output_limit,current_limit, 
	       error_limit, rate_divisor, deadband_compensation, 0);
  /*
    ServoSetGain(ix,
    Kp,
    Kd,
    Ki,
    integration_limit,
    output_limit,
    current_limit,
    error_limit,
    rate_divisor,
    deadband_compensation
    );
  */
    
}

void nmc_set_pid_parameters(nmc_port *port, uint ix, uint Kp, uint Ki, uint Kd) {
  nmc_set_full_pid_parameters(port, ix, Kp, Ki, Kd, 0, 255, 0, 4000, 1, 0);
}

void nmc_motors_on(nmc_port *port, int ix) {
  nmc_motor_stop(port, ix, AMP_ENABLE | MOTOR_OFF);  //enable amp
  nmc_motor_stop(port, ix, AMP_ENABLE | STOP_ABRUPT);//stop at current pos.
  nmc_reset(port, ix);                    //reset the position counter to 0
}

#define DIV 6

void nmc_set_position(nmc_port *port, int ix, int value, long speed) {
  nmc_servo_load_traj
    (port, ix, 		
     LOAD_POS | LOAD_VEL | LOAD_ACC |  ENABLE_SERVO | START_NOW, 
     value,		
     speed*10, 	
     100000, 	       
     0,
     true
     );
}

void nmc_wait_position(nmc_port *port, int ix) {
  // Poll status byte until MOVE_DONE goes high.
  do {
    nmc_no_op(port, ix);	//poll controller to get current status data
  }
  while ( !(port->modules[ix-1].status_byte & MOVE_DONE) );  
}

long nmc_get_position(nmc_port *port, int ix, bool request) {
  if (request) {
    //Cause PIC-SERVO controller to send back position data with each command
    //Position data will also be sent back with this command
    //nmc_define_status(port, ix, SEND_POS);
    nmc_send_read_status(port, ix, SEND_POS);
  }
  
  //Retrieve the position data from the local data structure
  return port->modules[ix-1].mod.servo.position;
}

void nmc_shutdown(nmc_port *port, char group) {
  nmc_hard_reset(port, group);
  close(port->fd);
  //free(port->modules);
}


void nmc_set_path(nmc_port *port, int ix, int *path, int length) {
  nmc_send_read_status(port, ix, SEND_POS | SEND_PERROR);
  port->modules[ix-1].mod.servo.last_point = 
    port->modules[ix-1].mod.servo.position +
    port->modules[ix-1].mod.servo.error;
  
  for (int i=0;i<length;i+=7) {
    int size;
    int *ptr = &(path[i]);
    
    if (i+7 > length)
      size = length-i;
    else
      size = 7;
    nmc_add_pathpoints(port, ix, size, ptr, P_30HZ);
  }
}

void nmc_set_group(nmc_port *port, int ix, int group, bool leader) {
  nmc_set_group_internal(port, ix, group, leader);
}

void nmc_start_path(nmc_port *port, int group, int leader) {
  nmc_start_path_internal(port, group, leader);
}

int ct=0;
float time=0;

void nmc_send_pwm(nmc_port *port, int ix, int pwm, bool wait) {
  int mode = LOAD_PWM | START_NOW;
  if (pwm < 0) {
    mode |= REVERSE;
    pwm = -pwm;
  }

  nmc_servo_load_traj(port, ix, mode, 0, 0, 0, pwm, wait);
}

void nmc_wait(nmc_port *port, int ix) {
  nmc_read_status(port, ix);
}

int nmc_move_done(nmc_port *port, int addr) {
  printf("%d %d\n", port->fd, addr);
  print_status(port->modules[addr-1].status_byte);
  return port->modules[addr-1].status_byte & MOVE_DONE;
}

int nmc_no_op(nmc_port *port, char addr) {
  nmc_no_op(port, addr);
}
